﻿using AppFramework.Logger;
using PerpheralDevice.Units;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.IO;
using System.Linq;
using System.Runtime.CompilerServices;
using System.Text;
using System.Threading.Tasks;
using System.Windows;
using YamlDotNet.Serialization;

namespace PerpheralDevice.LeadShine
{
    public class DMCMotor : Motor {
        private readonly ILogger _logger = ILogger.Default;

        #region 本地回零处理
        private void WaitForReseting() {
            int tempPosPulse = this.PosCommand;
            int tempDullTimes = 0;
            do {
                Task.Delay(50).Wait();
                if (!this.mfIsReseting) {
                    this.Stop();
                } else {
                    if (Math.Abs(this.PosCommand - tempPosPulse) < 10) {
                        if (tempDullTimes++ > 3) {
                            this.Stop();
                            throw new AppTException<DMCMotor>($"伺服运动的脉冲信号意外停止({this.mfCardNO},{this.AxisID})");
                        }
                    } else {
                        tempDullTimes = 0;
                        tempPosPulse = this.PosCommand;
                    }
                }                
            } while (this.IsMoving);
        }
        /// <summary>
        /// 一次回零
        /// </summary>
        /// <returns></returns>
        private bool ResetOnce() {
            //Serilog.Log.Debug($"运动轴({this.mCardNO},{this.mAxisNO})一次回零加回找...");

            //设置回零模式
            //home_dir回零方向，0：负向，1：正向
            //vel_mode回零速度模式, 0：低速回零(即以本指令前面的dmc_set_profile函数设置的起始速度运行)， 1：高速回零
            //mode回零模式, 0：一次回零 1：一次回零加回找 2：二次回零 3：原点加同向EZ 4：单独记一个EZ 5：原点加反向EZ 6：原点锁存
            //7：原点锁存加同向EZ 8：单独记一个EZ锁存 9：原点锁存加反向EZ 10.一次限位回零 11. 一次限位回零加反找 12. 二次限位回零
            ushort homeDir = (ushort)(this.VelWork > 0 ? 0 : 1);
            LeadShine.LTDMC.dmc_set_homemode(this.mfCardNO, this.mfAxisNO, homeDir, 0, 1, 0);
            //启动回零
            LeadShine.LTDMC.dmc_home_move(this.mfCardNO, this.mfAxisNO);

            this.WaitForReseting();

            // state 回零执行状态，1：回零完成，0：回零未完成
            ushort state = 0;
            LeadShine.LTDMC.dmc_get_home_result(this.mfCardNO, this.mfAxisNO, ref state);
            LeadShine.LTDMC.dmc_get_position(this.mfCardNO, this.mfAxisNO);

            //Serilog.Log.Debug($"运动轴({this.mCardNO},{this.mAxisNO})一次回零加回找({state == 1}");
            LeadShine.LTDMC.dmc_set_position(this.mfCardNO, this.mfAxisNO, 0);
            LeadShine.LTDMC.dmc_set_encoder(this.mfCardNO, this.mfAxisNO, 0);

            return state == 1;
        }
        /// <summary>
        /// 原点加同向EZ
        /// </summary>
        /// <param name="resetVel"></param>
        /// <exception cref="AppException.AppTException{DMC5410AAxis}"></exception>
        private bool ResetWithEZ() {
            //Serilog.Log.Debug($"运动轴({this.mfCardNO},{this.mAxisNO})原点加反向EZ...");

            //设置回零模式
            //home_dir回零方向，0：负向，1：正向
            //vel_mode回零速度模式, 0：低速回零(即以本指令前面的dmc_set_profile函数设置的起始速度运行)， 1：高速回零
            //mode回零模式, 0：一次回零 1：一次回零加回找 2：二次回零 3：原点加同向EZ 4：单独记一个EZ 5：原点加反向EZ 6：原点锁存
            //7：原点锁存加同向EZ 8：单独记一个EZ锁存 9：原点锁存加反向EZ 10.一次限位回零 11. 一次限位回零加反找 12. 二次限位回零
            ushort homeDir = (ushort)(this.VelWork > 0 ? 0 : 1);
            LeadShine.LTDMC.dmc_set_homemode(this.mfCardNO, this.mfAxisNO, homeDir, 0, 5, 0);

            //启动回零
            LeadShine.LTDMC.dmc_home_move(this.mfCardNO, this.mfAxisNO);

            this.WaitForReseting();

            // state 回零执行状态，1：回零完成，0：回零未完成
            ushort state = 0;
            LeadShine.LTDMC.dmc_get_home_result(this.mfCardNO, this.mfAxisNO, ref state);

            LeadShine.LTDMC.dmc_get_position(this.mfCardNO, this.mfAxisNO);
            //Serilog.Log.Debug($"运动轴({this.mCardNO},{this.mAxisNO})原点加反向EZ({state == 1})");
            LeadShine.LTDMC.dmc_set_position(this.mfCardNO, this.mfAxisNO, 0);
            LeadShine.LTDMC.dmc_set_encoder(this.mfCardNO, this.mfAxisNO, 0);

            return state == 1;
        }
        #endregion

        private bool mfIsReseting = false;

        #region 伺服应用操作

        public override Motor Reset() {
            this._logger.Info($"{this.GetType().FullName}:{this.PerpDev.PerpDevName}=>{nameof(Reset)}({this.UnitID})");

            if (this.PerpDev.IsMocking) {
                this.MotorState = MotorState.Waiting;
                return this;
            }

            if (!this.mfIsReseting) {
                this.mfIsReseting = true;
                if (this.mfIsReseting) {
                    if (this.ResetOnce()) {
                        if (this.mfIsReseting) {
                            this.MotorState = this.ResetWithEZ() ? MotorState.Waiting : MotorState.Unkown;
                        }
                    }
                    this.mfIsReseting = false;
                }
            }

            while (this.mfIsReseting) {
                Task.Delay(50).Wait();
            }

            return this;
        }

        public override Motor ReachRel(int posDst) {
            if (this.PerpDev.IsMocking) return this;
            this._logger?.Debug($"运动轴({this.mfCardNO},{this.mfAxisNO},指令脉冲位置: {this.PosCommand},编码器位置: {this.PosEncoder})前往({posDst})开始...");

            LeadShine.LTDMC.dmc_pmove(this.mfCardNO, this.mfAxisNO, posDst, 0); // 定长相对运动
            while (this.IsMoving) {
                Task.Delay(50).Wait();
            }
            this._logger?.Debug($"运动轴({this.mfCardNO},{this.mfAxisNO},指令脉冲位置: {this.PosCommand},编码器位置: {this.PosEncoder})前往({posDst})结束");

            return this;
        }

        public override Motor ReachAbs(int posDst) {
            if (this.PerpDev.IsMocking) return this;
            this._logger?.Debug($"运动轴({this.mfCardNO},{this.mfAxisNO},指令脉冲位置: {this.PosCommand},编码器位置: {this.PosEncoder})前往({posDst})开始...");
            
            LeadShine.LTDMC.dmc_pmove(this.mfCardNO, this.mfAxisNO, posDst, 1); // 定长绝对运动
            while (this.IsMoving) {
                Task.Delay(50).Wait();
            }
            this._logger?.Debug($"运动轴({this.mfCardNO},{this.mfAxisNO},指令脉冲位置: {this.PosCommand},编码器位置: {this.PosEncoder})前往({posDst})结束");
            
            return this;
        }

        public override Motor DoMoving() {
            this._logger.Info($"{this.GetType().FullName}:{this.PerpDev.PerpDevName}=>{nameof(DoMoving)}({this.UnitID})");
            if (this.PerpDev.IsMocking) return this;
            
            ushort dir = (ushort)(this.VelWork > 0 ? 1 : 0);
            LeadShine.LTDMC.dmc_vmove(this.mfCardNO, this.mfAxisNO, dir);

            return this;
        }

        public override Motor Stop() {
            this._logger.Info($"{this.GetType().FullName}:{this.PerpDev.PerpDevName}=>{nameof(Stop)}({this.UnitID})");
            if (this.PerpDev.IsMocking) return this;
            do {
                LeadShine.LTDMC.dmc_stop(this.mfCardNO, this.mfAxisNO, 0);
            } while (this.IsMoving);

            return this;
        }

        public override Motor Scram() {
            this.MotorState = MotorState.Unkown;
            this.mfIsReseting = false;

            do {
                LeadShine.LTDMC.dmc_stop(this.mfCardNO, this.mfAxisNO, 1);
            } while (this.IsMoving);

            return this;
        }

        #endregion

        #region 伺服速度属性

        private double mfVelBoot; //启动速度
        private double mfVelWork; //最大速度
        private double mfVelStop; //停止速度
        private double mfTimeAcc; //加速时间
        private double mfTimeDec; //减速时间
        private DMCMotor getVel() {
            LTDMC.dmc_get_profile(this.mfCardNO, this.mfAxisNO, ref this.mfVelBoot, ref this.mfVelWork, ref this.mfTimeAcc, ref this.mfTimeDec, ref this.mfVelStop);
            return this;
        }

        public override int VelBoot => (int)this.getVel().mfVelBoot;
        public override Motor VelBootSet(int velBoot) {
            if (this.VelBoot != velBoot) {
                LTDMC.dmc_set_profile(this.mfCardNO, this.mfAxisNO, velBoot, this.mfVelBoot, this.mfTimeAcc, this.mfTimeDec, this.mfVelStop);
            }
            return this;
        }

        public override int VelWork =>(int)this.getVel().mfVelWork;
        public override Motor VelWorkSet(int velWork) {
            if (this.VelWork != velWork) {
                LTDMC.dmc_set_profile(this.mfCardNO, this.mfAxisNO, this.mfVelBoot, velWork, this.mfTimeAcc, this.mfTimeDec, this.mfVelStop);
            }
            return this;
        }

        public override int VelStop => (int)this.getVel().mfVelStop;
        public override Motor VelStopSet(int velStop) {
            if (this.VelStop != velStop) {
                LTDMC.dmc_set_profile(this.mfCardNO, this.mfAxisNO, this.mfVelBoot, this.mfVelWork, this.mfTimeAcc, this.mfTimeDec, velStop);
            }
            return this;
        }

        public override int VelAcc => default;
        public override Motor VelAccSet(int velAcc) {
            return this;
        }

        public override int VelDec => default;
        public override Motor VelDecSet(int velDec) {
            return this;
        }

        public override double TimeDec => this.getVel().mfTimeDec;
        public override Motor TimeDecSet(double timeDec) {
            if (this.TimeDec != timeDec) {
                LTDMC.dmc_set_profile(this.mfCardNO, this.mfAxisNO, this.mfVelBoot, this.mfVelWork, this.mfTimeAcc, timeDec, this.mfVelStop);
            }
            return this;
        }
                
        public override double TimeAcc => this.getVel().mfTimeAcc;
        public override Motor TimeAccSet(double timeAcc) {
            if (this.mfTimeAcc != timeAcc) {
                LTDMC.dmc_set_profile(this.mfCardNO, this.mfAxisNO, this.mfVelBoot, this.mfVelWork, timeAcc, this.mfTimeDec, this.mfVelStop);
            }
            return this;
        }

        public override Motor ServoEnable() {
            throw new NotImplementedException();
        }

        public override Motor ServoDisable() {
            throw new NotImplementedException();
        }

        public override bool DirMoveIsForward => throw new NotImplementedException();
        public override Motor DirMoveIsForwardSet(bool isForward) {

            return this;
        }
        #endregion

        private readonly ushort mfCardNO;
        private readonly ushort mfAxisNO;
        /// <summary>
        /// 运动卡ID
        /// </summary>
        public object BoardID { get; }
        /// <summary>
        /// 运动轴序号
        /// </summary>
        public object AxisID { get; }

        public override bool IsMoving => LeadShine.LTDMC.dmc_check_done(this.mfCardNO, this.mfAxisNO) == 0;
        /// <summary>
        /// 运动结束后, 检测编码器到位(用法详见章节 7.10 检测轴到位状态功能的实现)
        /// </summary>
        public override bool IsMovedOK => LeadShine.LTDMC.dmc_check_success_encoder(this.mfCardNO, this.mfAxisNO) == 1;
        /// <summary>
        /// 指令脉冲位置
        /// </summary>
        public override int PosCommand => LeadShine.LTDMC.dmc_get_position(this.mfCardNO, this.mfAxisNO);
        /// <summary>
        /// 编码器位置
        /// </summary>
        public override int PosEncoder => LeadShine.LTDMC.dmc_get_encoder(this.mfCardNO, this.mfAxisNO);

        public override string UnitID { get; }

        public override MotorStatus MotorStatus => throw new NotImplementedException();

        /// <summary>
        /// 运动轴 构造
        /// </summary>
        /// <param name="motionCard">运动卡</param>
        /// <param name="axisNO">运动轴序号</param>
        public DMCMotor(PerpDev perpDev, int axisNO) {
            this.PerpDev = perpDev;
            this.UnitID = axisNO.ToString(); 
        }

    }
}
